/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *  srv.c - routines to interface with the SRV-1 Blackfin robot.
 *    modified from main.c - main control loop for SRV-1 robot
 *    Copyright (C) 2005-2007  Surveyor Corporation
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details (www.gnu.org/licenses)
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <cdefBF537.h>
#include "config.h"
#include "uart.h"
#include "i2c.h"
#include "ov9655.h"
#include "camera.h"
#include "jpeg.h"
#include "xmodem.h"
#include "stm_m25p32.h"
#include "font8x8.h"
#include "malloc.h"
#include "string.h"
#include "robochallenge.h"
#include "print.h"

#include "srv.h"

/* Size of frame */
unsigned int imgWidth, imgHeight;

/* Version */
unsigned char version_string[] = "SRV-1 Blackfin Robochallenge Edition - "  __TIME__ " - " __DATE__ ;

/* Frame count output string */
unsigned char frame[] = "000-deg 000-f 000-d 000-l 000-r";
//unsigned char frame[] = "frame     ";

/* Camera globals */
unsigned int quality, framecount, ix;
unsigned int segmentation_flag, edge_detect_flag, frame_diff_flag, robochallenge;
unsigned int edge_thresh;
unsigned char *output_start, *output_end; /* Framebuffer addresses */
unsigned int image_size; /* JPEG image size */
char imgHead[11]; /* image frame header for I command */
short hhpel[] = {0, -1, 0, 1, -1, 1, -1, 0, 1};
short vhpel[] = {0, -1, -1, -1, 0, 0, 1, 1, 1};

/* IMU globals */
int x_acc, x_acc0, x_center;
int y_acc, y_acc0, y_center;

/* random number generator globals */
unsigned int deadbeef_seed;
unsigned int deadbeef_beef = 0xdeadbeef;

/* General globals */
unsigned char *cp;
unsigned int i, j; // Loop counter.
unsigned int silent_console;

/* xmodem globals */
int err;

void init_io() {
    *pPORTGIO_DIR = 0x0300;   // LEDs (PG8 and PG9)
    *pPORTH_FER = 0x0000;     // set for GPIO
    *pPORTHIO_DIR |= 0x0040;  // set PORTH6 to output for serial flow control
    *pPORTHIO = 0x0000;       // set output low 
    *pPORTHIO_INEN |= 0x000D; // enable inputs: Matchport RTS0 (H0), battery (H2), master/slave (H3)
    silent_console = 0;
}

/* reset CPU */
void reset_cpu() {
    asm(
    "p0.l = 0x0100; "
    "p0.h = 0xFFC0; "
    "r0.l = 0x0007; "
    "w[p0] = r0; "
    "ssync; "
    "p0.l = 0x0100; "
    "p0.h = 0xFFC0; "
    "r0.l = 0x0000; "
    "w[p0] = r0; "
    "ssync; "
    "raise 1; ");                        
}

/* clear SDRAM */
void clear_sdram() {
  for (cp=(unsigned char *)0x00100000; cp<(unsigned char *)0x02000000; cp++) {
    *cp = 0;
  }
}

/* SRV-1 Firmware Version Request
   Serial protocol char: V */
void serial_out_version () {
    printf("##Version - %s\n\r", version_string); 
}

/* Get current time
   Serial protocol char: t */
void serial_out_time () {
	printf("##time - millisecs:  %d\n\r", readRTC());
}

/* Dump flash buffer to serial
   Serial protocol char: z-d */
void serial_out_flashbuffer () {
    printf("##zdump: \n\r");
    cp = (unsigned char *)FLASH_BUFFER;
    for (i=0; i<0x10000; i++) {
        if (*cp == 0)
            return;
        putchar(*cp++);
    }
}

void led0_on() {
    *pPORTGIO = 0x0100;  // turn on LED0
}

void led1_on() {
    *pPORTGIO = 0x0200;  // turn on LED1
}

void set_edge_thresh () {
    unsigned char ch;
    ch = getch();
    edge_thresh = (unsigned int)(ch & 0x0f) * 500;
    if (!silent_console) {
        printf("#T\n\r");
    }
}

void disable_frame_diff() {  // disables both frame differencing and color segmentation
    frame_diff_flag = 0;
    segmentation_flag = 0;
	edge_detect_flag = 0;
    if (!silent_console)
        printf("#G\n\r");
}

void grab_reference_frame () {
    move_image((unsigned char *)DMA_BUF1, (unsigned char *)DMA_BUF2, 
            (unsigned char *)FRAME_BUF2, imgWidth, imgHeight); 
}

/*  compute frame difference between two frames 
     U and V are computed by U1 + 128 - U2
     Y is computed as abs(Y1 - Y2) 
     fcur is current frame
     fref is reference frame*/
void compute_frame_diff(unsigned char *fcur, unsigned char *fref, int w1, int h1) {
    int ix, ipix;
    
    ipix = w1*h1*2;
    for (ix=0; ix<ipix; ix+=2) {
        fcur[ix] = (unsigned char)((unsigned int)fcur[ix] - (unsigned int)fref[ix] + 128);
        if (fcur[ix+1] < fref[ix+1])
            fcur[ix+1] = fref[ix+1] - fcur[ix+1];
        else
            fcur[ix+1] = fcur[ix+1] - fref[ix+1];
    }
}

/* JPEG compress and send frame captured by grab_frame()
   Serial protocol char: I */
void send_frame () {
    unsigned char ch;
    output_start = (unsigned char *)JPEG_BUF;
    output_end = encode_image((unsigned char *)FRAME_BUF, output_start, quality, 
            imgWidth, imgHeight); 
    image_size = (unsigned int)(output_end - output_start);

    led1_on();
    framecount++;

    imgHead[6] = (unsigned char)(image_size & 0x000000FF);
    imgHead[7] = (unsigned char)((image_size & 0x0000FF00) >> 8);
    imgHead[8] = (unsigned char)((image_size & 0x00FF0000) >> 16);
    imgHead[9] = 0x00;
    for (i=0; i<10; i++) {
        while (*pPORTHIO & 0x0001)  // hardware flow control
            continue;
        putchar(imgHead[i]);
    }
    cp = (unsigned char *)JPEG_BUF;
    for (i=0; i<image_size; i++) 
        putchar(*cp++);

    while (getchar(&ch)) {
        // flush input 
        continue;
    }
}

/* Camera initial setup */
void camera_setup () {
    frame_diff_flag = 0;
    segmentation_flag = 0;
    imgWidth = 320;
    imgHeight = 240;
    strcpy(imgHead, "##IMJ5    ");
    i2cwrite(0x30, ov9655_qvga, sizeof(ov9655_qvga)>>1, SCCB_ON);
    delayMS(100);
    i2cwrite(0x30, ov9655_qvga, sizeof(ov9655_qvga)>>1, SCCB_ON);
    camera_init((unsigned char *)DMA_BUF1, (unsigned char *)DMA_BUF2, imgWidth, imgHeight);
    camera_start();

    /* Initialise camera-related globals */
    framecount = 0;
    quality = 4; // Default JPEG quality - range is 1-8 (1 is highest)
}

/* Refactored out, code to reset the camera after a frame size change. */
void camera_reset (unsigned int width) {
    imgWidth = width;
    if (width == 160) {
        imgHeight = 120;
        strcpy(imgHead, "##IMJ3    ");
        camera_stop();
        i2cwrite(0x30, ov9655_qqvga, sizeof(ov9655_qqvga)>>1, SCCB_ON);
        if (!silent_console)
            printf("#a\n\r");
    } else if (width == 320) {
        imgHeight = 240;
        strcpy(imgHead, "##IMJ5    ");
        camera_stop();
        i2cwrite(0x30, ov9655_qvga, sizeof(ov9655_qvga)>>1, SCCB_ON);
        if (!silent_console)
            printf("#b\n\r");
    } else if (width == 640) {
        imgHeight = 480;
        strcpy(imgHead, "##IMJ7    ");
        camera_stop();
        i2cwrite(0x30, ov9655_vga, sizeof(ov9655_vga)>>1, SCCB_ON);
        if (!silent_console)
            printf("#c\n\r");
    } else if (width == 1280) {
        imgHeight = 1024;
        strcpy(imgHead, "##IMJ9    ");
        camera_stop();
        i2cwrite(0x30, ov9655_sxga, sizeof(ov9655_sxga)>>1, SCCB_ON);
        if (!silent_console)
            printf("#A\n\r");
    }
    camera_init((unsigned char *)DMA_BUF1, (unsigned char *)DMA_BUF2, imgWidth, imgHeight);
    camera_start();
}

/* Change image quality.
   Serial protocol char: q */
void change_image_quality () {
    unsigned char ch;
    ch = getch();
    quality = (unsigned int)(ch & 0x0f);
    if (quality < 1) {
        quality = 1;
    } else if (quality > 8) {
        quality = 8;
    }
    if (!silent_console) {
		printf("##quality - %c\n\r", ch);
    }
}

/* XModem Receive.
   Serial protocol char: X */
void xmodem_receive () {
  err = xmodemReceive((unsigned char *)FLASH_BUFFER, 131072);
  if (err < 0) {
		printf("##Xmodem receive error: %d\n\r", err);
  } else {
      printf("##Xmodem success. Count: %d\n\r");
  }
}

/* Clear flash buffer
   Serial protocol char: z-c */
void clear_flash_buffer () {
    for (ix = FLASH_BUFFER; ix < (FLASH_BUFFER  + 0x00020000); ix++)
      *((unsigned char *)ix) = 0;   // clear the read buffer
    printf("##zclear buffer\n\r");
}

/* crc flash buffer using crc16_ccitt()
   Serial protocol char: z-C */
void crc_flash_buffer () {
    unsigned int ix;
    ix = (unsigned int)crc16_ccitt((void *)FLASH_BUFFER, 0x0001fff8);  // don't count last 8 bytes
	printf("##zCRC: 0x%x\n\r", ix);
}

/* Read user flash sector into flash buffer
   Serial protocol char: z-r */
void read_user_flash () {
    for (ix = FLASH_BUFFER; ix < (FLASH_BUFFER  + 0x00010000); ix++)
      *((unsigned char *)ix) = 0;   // clear the read buffer
    ix = spi_read(FLASH_SECTOR, (unsigned char *)FLASH_BUFFER, 0x00010000);
	printf("##zread count: %d\n\r", ix);
}

/* Write user flash sector from flash buffer
   Serial protocol char: z-w */
void write_user_flash () {
    ix = spi_write(FLASH_SECTOR, (unsigned char *)FLASH_BUFFER, 
        (unsigned char *)(FLASH_BUFFER + 0x00010000), 0x00010000);
    printf("##zwrite count: %d\n\r", ix);
}

/* Write boot flash sectors (1-2) from flash buffer
   Serial protocol char: z-Z */
void write_boot_flash () {
    cp = (unsigned char *)FLASH_BUFFER;
    if (cp[1] != 0x00 && cp[2] != 0x80 && cp[3] != 0xFF) {
        printf("##zZ boot image - invalid header\n\r");
        return;
    }                        
    ix = spi_write(BOOT_SECTOR, (unsigned char *)FLASH_BUFFER, 
        (unsigned char *)(FLASH_BUFFER + 0x00020000), 0x00020000);
    printf("##zZ boot image write count: %d\n\r", ix);
}

/* Initialise the Real-time Clock */
void initRTC() {
    *pRTC_ICTL = 0;  // disable interrupts
    SSYNC;
    *pRTC_PREN = 0;  // disable prescaler - clock counts at 32768 Hz
    SSYNC;
    *pRTC_STAT = 0;  // clear counter
    SSYNC;
}

/* Read the RTC counter, returns number of milliseconds since reset */
int readRTC() {     
    int i1, i2;
    i1 = *pRTC_STAT;
    i2 = (i1 & 0x0000003F) + (((i1 >> 6) & 0x0000003F) * 60) +  
        (((i1 >> 12) & 0x0000001F) * 3600) + (((i1 >> 17) & 0x00007FFF) * 86400);
    return (i2 / 33);  // converts tick count to milliseconds
                       //    32,768 / 32.77 = 1,000
}

/* Clear the RTC counter value */
void clearRTC() {
    *pRTC_STAT = 0;
    SSYNC;
}

void delayMS(int delay) {  // delay up to 100000 millisecs (100 secs)
    int i0;

    if ((delay < 0) || (delay > 100000))
        return;
    i0 = readRTC();
    while (readRTC() < (i0 + delay))
        continue;
}

/* infamous 0xDEADBEEF pseudo-random number generator by 
       Robbert Haarman http://inglorion.net 
   returns number between 0x0000 and 0xFFFF */
   
int rand() {
    if (deadbeef_beef == 0xdeadbeef)  // initialize 
        deadbeef_seed = readRTC();
    deadbeef_seed = (deadbeef_seed << 7) ^ ((deadbeef_seed >> 25) + deadbeef_beef);
    deadbeef_beef = (deadbeef_beef << 7) ^ ((deadbeef_beef >> 25) + 0xdeadbeef);
    return (deadbeef_seed & 0x0000FFFF);  // return rand number from 0x0000 to 0xFFFF
}

unsigned int isqrt(unsigned int val) {
    unsigned int temp, g=0, b = 0x8000, bshft = 15;
    do {
        if (val >= (temp = (((g << 1) + b)<<bshft--))) {
           g += b;
           val -= temp;
        }
    } while (b >>= 1);
    return g;
}
